﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Diagnostics;
using NIP.ControllerInterface;
using NIP.DynamicControl.Environ;

namespace NIP.DynamicControl
{
    public class RobotManager
    {

        public ControllerInterface.UsbcController controllerObject;
        public NIP.DynamicControl.Environ.PathPlanning PathPlanningObject { set; get; }
        public NIP.DynamicControl.InverseKinematics.Inv_K Inv_KObject { set; get; }
        public NIP.DynamicControl.PID.JointPID PIDObject;
        public Thread PIDthread;
        public Thread PPthread;

        
        
        public bool isHoming = false;
        public bool PPinitialised = false;
        public bool PIDinitialised = false;
        public bool InMotion = true;

        public bool GripClosed
        {
            get
            {
                short gripperOpenPercent = 0;
                short gripperOpenMM = 0;
                controllerObject.GetJaw(ref gripperOpenPercent, ref gripperOpenMM);
                //Thread.Sleep(1000);
                bool result = (GripperStatusHelper.TranslateFromJawPercentToGripper(gripperOpenPercent) != GripperStatus.OPEN) ?
                    true : false;
                Debug.WriteLine("GripClosed - grip closed?: " + result);
                return result;
            }
        }

       

               

        public RobotManager()
        {
            Console.WriteLine("RobotManager object: starting");
            Inv_KObject = new NIP.DynamicControl.InverseKinematics.Inv_K();
            PathPlanningObject = new NIP.DynamicControl.Environ.PathPlanning();
            PPthread = new Thread(PathPlanningloop);
            PPthread.Priority = ThreadPriority.BelowNormal;
            PPthread.Start();
            Thread.Sleep(100);
            

            controllerObject = new ControllerInterface.UsbcController();
            //!Watching functions
            controllerObject.HomingStatusChanged += new EventHandler<ControllerInterface.HomingStatusEventArgs>(HomingStatusChanged_Handler);
            
            
            
            controllerObject.Initialization(1, 92);
            Thread.Sleep(2000);
            controllerObject.Control((byte)'&', true);

            
          
            PIDObject = new NIP.DynamicControl.PID.JointPID(controllerObject);
            //PIDthread = new Thread(PIDObject.PIDloopOld);
            PIDthread = new Thread(PIDloop);
            PIDthread.Priority = ThreadPriority.AboveNormal;
            
            //PIDthread.Start();
            Thread.Sleep(1000);
            //Console.WriteLine("Gripper Closed ? " + PIDObject.GripClosed);
            //set current position to desired position and set ppinitialised to true;

            
            
            

        }

        public PathPoint getDesiredPos(double t_incr)
        {
            return PathPlanningObject.getDesiredPos(t_incr);
            //return PathPlanningObject.desiredPosTemp;
        }
          

        private void PIDloop()
        {
            Console.WriteLine("PID thread: Initialising");
            double dt;
            PIDObject.PIDinit();
            PIDinitialised = true;
            Console.WriteLine("PID thread: Waiting for Path Planning thread");
            while (!PPinitialised)
            {
                Thread.Sleep(100);
            }
            Console.WriteLine("PID thread: Initialised");
            while (true)
            {
                PIDObject.shouldStop() ;
                PIDObject.getCurrPos();
                
                dt = PIDObject.getDt();
                if (PPinitialised)
                {
                    PIDObject.setDesiredPos(getDesiredPos(dt)) ;
                }
                else
                {
                    PIDObject.setDesiredPos(PIDObject.CurrentPos.get()) ;
                }
                PIDObject.calculateGains() ;
                PIDObject.writeTorque() ;
                InMotion = PIDObject.InMotion;
                PathPlanningObject.InMotion = InMotion;
                PIDObject.loopDelay() ;
                //if (PIDObject.OpenGripper || PIDObject.CloseGripper)
                //    PIDObject.setGripper();
            }
        }

        private void PathPlanningloop()
        {
            Console.WriteLine("Path Planning thread: waiting for PID thread");
            while (!PIDinitialised) //wait for PID to initialise and set current pos.
            {
                Thread.Sleep(100);
            }
            //plan path of one step, from current to current.
            Console.WriteLine("Path Planning thread: creating initial path");
            PathPlanningObject.setInitialPath(PIDObject.CurrentPos.get());
            Console.WriteLine("Path Planning thread: Initialised");
            PPinitialised = true;

            while (true)
            {
                Thread.Sleep(100);
                //will do something eventually;
            }

        }


        public void HomingStatusChanged_Handler(object sender, ControllerInterface.HomingStatusEventArgs args)
        {
           switch (args.HomingStatus)
           {
               case HomingStatus.Started:
                   isHoming = true;
                   System.Diagnostics.Debug.WriteLine("Homing Started");
                   
                   break;
               case HomingStatus.Ended:
                   isHoming = false;
                   System.Diagnostics.Debug.WriteLine("Homing Ended");
                   break;
               case HomingStatus.Error:
                throw(new System.Exception("Error in Homing "));
               default:
                break;
           }
        }
        /// <summary>
        /// Check and disable PID before any action
        /// </summary>
        /// <param name="EnableControl"> After Deactivate PID should Control be turned on?</param>
        /// <returns></returns>
        public bool PreActionPIDCheck(bool EnableControl = false)
        {
            if (PIDObject.IsActive())
            {
                Console.WriteLine("PID was active");
                PIDObject.RequestStop();
                while (PIDObject.IsActive()) ;
                //Thread.Sleep(500);
                Console.WriteLine("JointPID thread disabling");
                while (PIDObject.IsActive()) ; // double check!
                //Thread.Sleep(500);

                if (EnableControl)
                {
                    controllerObject.Control((byte)Axis.All,true);
                    Thread.Sleep(2000);
                }
                return (!PIDObject.IsActive()); // return true means NOT ACTIVE
            }
            else return true;
        }

        public bool PostActionPIDCheck()
        {
            if (!PIDObject.IsActive())
            {
                Console.WriteLine("JointPID thread reactivating");
                PIDObject.RequestStart();
                while (!PIDObject.IsActive()) ;
                Console.WriteLine("JointPID thread reactivating 2");
                return PIDObject.IsActive();
            }
            else return true;
        }

        public void ToggleGripper()
        {
            bool initiallyActive = PIDObject.IsActive();

            if (!(PreActionPIDCheck(false)))
                return; //failed

            if (GripClosed)
            {
                //PIDObject.OpenGripper = true;
                controllerObject.OpenGripper();
                Console.WriteLine("Gripper Closed ? " + GripClosed);
                //! Disabled due to use of GetJaw()
                // robotObject.GripClosed = false;
            }
            else
            {
                //PIDObject.CloseGripper = true;
                controllerObject.CloseGripper();
                Console.WriteLine("Gripper Closed ? " + GripClosed);
                //! Disabled due to use of GetJaw()
                //robotObject.GripClosed = true;
            }
            Thread.Sleep(2000);
            if (initiallyActive) PostActionPIDCheck();
        }
    }
}
